#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include <algorithm>

#include "angles/angles.h"
#include "matplotlibcpp.h"
#include "agv_model.hpp"
#include "lqr_solver.hpp"

namespace plt = matplotlibcpp;
using namespace std;

vector<string> split(const string &line, char delimiter)
{
    vector<string> tokens;
    string token;
    istringstream tokenStream(line);
    while (getline(tokenStream, token, delimiter))
    {
        tokens.push_back(token);
    }
    return tokens;
}

WayPoint getTargetPoint(RobotState cur_pose,
                        vector<double> &path_x,
                        vector<double> &path_y,
                        bool backward = false)
{
    // 1 计算最近点坐标
    int size = path_x.size();
    vector<double> dist_list;
    for (int i = 0; i < size; i++)
    {
        double dist = hypot(path_x[i] - cur_pose.x, path_y[i] - cur_pose.y);
        dist_list.push_back(dist);
    }
    auto min_t = min_element(dist_list.begin(), dist_list.end());
    int index = distance(dist_list.begin(), min_t) + 3;
    index = clamp(index, 0, size - 1);

    // 2 计算目标路径点相关信息
    double dx, dy, ddx, ddy;
    if (index == 0)
    {
        dx = path_x[1] - path_x[0];
        dy = path_y[1] - path_y[0];
        ddx = path_x[2] + path_x[0] - 2.0 * path_x[1];
        ddy = path_y[2] + path_y[0] - 2.0 * path_y[1];
    }
    else if (index == size - 1)
    {
        dx = path_x[index] - path_x[index - 1];
        dy = path_y[index] - path_y[index - 1];
        ddx = path_x[index] + path_x[index - 2] - 2.0 * path_x[index - 1];
        ddy = path_y[index] + path_y[index - 2] - 2.0 * path_y[index - 1];
    }
    else
    {
        dx = (path_x[index + 1] - path_x[index - 1]) / 2.0;
        dy = (path_y[index + 1] - path_y[index - 1]) / 2.0;
        ddx = path_x[index + 1] + path_x[index - 1] - 2.0 * path_x[index];
        ddy = path_y[index + 1] + path_y[index - 1] - 2.0 * path_y[index];
    }
    double denominator = std::pow(dx * dx + dy * dy, 1.5);
    if (denominator < 1e-6)
        denominator = 1e-6;

    WayPoint ref_point;
    ref_point.index = index;
    ref_point.x = path_x[index];
    ref_point.y = path_y[index];
    if (backward)
    {
        ref_point.yaw = angles::normalize_angle(atan2(dy, dx) + M_PI);
    }
    else
    {
        ref_point.yaw = atan2(dy, dx);
    }

    ref_point.k = (ddy * dx - ddx * dy) / denominator;
    return ref_point;
}

int main()
{
    // read data
    string filename = "../traj/backward.txt";
    ifstream infile(filename);

    if (!infile.is_open())
    {
        cerr << "can not open file" << filename << endl;
        return 1;
    }

    // deal data
    string line;
    vector<double> path_x, path_y;
    vector<double> traj_x, traj_y;
    vector<double> speed_list, angle_list, time_list;
    while (getline(infile, line))
    {
        vector<string> tokens = split(line, ',');
        path_x.push_back(stod(tokens[0]));
        path_y.push_back(stod(tokens[1]));
    }

    // var declare
    int loop = 0;
    double freq = 10.0;
    double length = 1.4;
    RobotState agv_pose;
    agv_pose.x = 0.1;
    agv_pose.y = -2.1;
    agv_pose.yaw = M_PI / 2;
    bool backward = true;
    LQRSolver lqr_solver(length, freq);
    AGVModule agv(length, freq, agv_pose);

    // calcu lqr traj
    int size = path_x.size();
    WayPoint ref_point = getTargetPoint(agv_pose, path_x, path_y, backward);
    Output last_cmd;
    last_cmd.linear = 0.0;
    last_cmd.angular = 0.0;
    while (loop < 200 && ref_point.index < size - 2)
    {
        ref_point.linear = -0.3;
        ref_point.angular = angles::shortest_angular_distance(agv_pose.yaw, ref_point.yaw);
        agv_pose.linear = last_cmd.linear;
        agv_pose.angular = last_cmd.angular;

        // save traj
        traj_x.push_back(agv_pose.x);
        traj_y.push_back(agv_pose.y);
        time_list.push_back(loop);
        speed_list.push_back(last_cmd.linear);
        angle_list.push_back(last_cmd.angular);

        // update traj cmd
        lqr_solver.initial(agv_pose, ref_point);
        Output cmd = lqr_solver.calcu();
        last_cmd.linear = last_cmd.linear + cmd.linear;
        last_cmd.angular = last_cmd.angular + cmd.angular;

        // update agv pose
        agv.updateState(last_cmd.linear, last_cmd.angular);
        agv_pose = agv.getPose();
        ref_point = getTargetPoint(agv_pose, path_x, path_y, backward);

        plt::clf();
        plt::subplot(3, 1, 1);
        plt::plot(path_x, path_y, "b");
        plt::plot(traj_x, traj_y, "g");
        plt::subplot(3, 1, 2);
        plt::plot(time_list, speed_list, "r");
        plt::subplot(3, 1, 3);
        plt::plot(time_list, angle_list, "r");
        plt::pause(0.03);
        loop = loop + 1;
    }

    plt::clf();
    plt::subplot(3, 1, 1);
    plt::plot(path_x, path_y, "-.b");
    plt::plot(traj_x, traj_y, "-.g");
    plt::subplot(3, 1, 2);
    plt::plot(time_list, speed_list, "r");
    plt::subplot(3, 1, 3);
    plt::plot(time_list, angle_list, "r");
    plt::show();
    return 0;
}
